/**
 * @file controller_node
 * @author yq
 * @brief
 * @version 0.1
 * @date 24-3-17
 *
 * @copyright Copyright (c) 2024
 */
#ifndef CATKIN_WORKSPACE_CONTROLLER_NODE_H
#define CATKIN_WORKSPACE_CONTROLLER_NODE_H

#include <cmath>
#include <iostream>
#include <cmath>
#include <string>
#include <memory>
#include <chrono>
#include <eigen3/Eigen/Dense>

// ros
#include "ros/ros.h"
#include "perception_msgs/PerceptionLocalization.h"
#include "perception_msgs/Trajectory.h"
#include "common_msgs/Control_Test.h"
#include "control_utils.h"
#include "lateral_control.h"
#include "longitudinal_control.h"

#include <nav_msgs/Path.h>                  //可视化轨迹
#include <geometry_msgs/PoseStamped.h>      //可视化轨迹
// #include <tf2_ros/transform_broadcaster.h>  //可视化车辆位姿
// #include <geometry_msgs/TransformStamped.h> //可视化车辆位姿
#include <visualization_msgs/Marker.h>      //可视化预测点、预瞄点、位姿
#include <geometry_msgs/Point.h>            //可视化预测点、预瞄点、位姿
#include <geometry_msgs/Pose.h>             //可视化车辆位姿
#include <eigen3/Eigen/src/Core/Matrix.h>

using Eigen::MatrixXd;

namespace vtd_pnc
{
    class ControllerNode
    {
    public:
        ControllerNode();
        void MainLoop();

    protected:
        double preview_time;

        // PID参数
        double distance_kp;
        double distance_ki;
        double distance_kd;
        double velocity_kp;
        double velocity_ki;
        double velocity_kd;

        // LQR参数
        Eigen::Matrix<double, 3, 3> Q;
        Eigen::Matrix<double, 2, 2> R;
        int max_iterations;
        double eps;

        bool reach_goal;
        VehicleConfig vehicle_config;
        ControlData control_cmd;

        CarState cur_pose;                                               // 车辆当前状态
        perception_msgs::TrajectoryPoint init_point_ref_trajectory_ptr;  // 轨迹起始点
        std::shared_ptr<perception_msgs::Trajectory> ref_trajectory_ptr; // 参考轨迹

        /***********************************Sbuscriber**************************************/
        ros::Subscriber cur_location_sub;   // 订阅`/cicv_location`, 获取车辆当前定位信息
        ros::Subscriber ref_trajectory_sub; // 订阅`/cicv_amr_trajectory`，获取路径规划轨迹点

        /***********************************Publisher**************************************/
        ros::Publisher control_cmd_pub;        // 发布控制命令
        ros::Publisher traj_rviz_pub;          // 可视化轨迹
        ros::Publisher preview_point_rviz_pub; // 可视化预瞄点
        ros::Publisher predict_point_rviz_pub;   // 可视化预测点
        ros::Publisher cur_pose_rviz_pub;      // 可视化车辆位姿

        /***********************************callback**************************************/
        void callbackLocation(const perception_msgs::PerceptionLocalization::ConstPtr &msg);
        void callbackRefTraj(const perception_msgs::Trajectory::ConstPtr &msg);

        /***********************************纵向控制**************************************/
        void initLonCon(ros::NodeHandle& n);

        /***********************************横向控制**************************************/
        void initLatCon(ros::NodeHandle& n);

        void initVehicleParameters(ros::NodeHandle& n); //初始化整车参数

    };

} // vtd_pnc

#endif // CATKIN_WORKSPACE_CONTROLLER_NODE_H
